#pragma once

#include <d3dx9math.h>
#include "Vector.h"
#include "Quaternion.h"

class Mat44 : public D3DXMATRIX
{
public:
	Mat44() : D3DXMATRIX() {}
	Mat44(D3DXMATRIX& mat) {memcpy(&m, &mat.m, sizeof(mat.m));}

	static const Mat44 g_identity;

	// Modifiers
	inline void SetPosition(const Vector3& pos)
	{
		m[3][0] = pos.x;
		m[3][1] = pos.y;
		m[3][2] = pos.z;
		m[3][3] = 1.f;
	}
	inline void SetPosition(const Vector4& pos)
	{
		m[3][0] = pos.x;
		m[3][1] = pos.y;
		m[3][2] = pos.z;
		m[3][3] = pos.w;
	}
	inline void SetScale(const Vector3& scale)
	{
		m[1][1] = scale.x;
		m[2][2] = scale.y;
		m[3][3] = scale.z;
	}

	// Accessors and Calc methods
	inline Vector3 GetPosition() const
	{
		return Vector3(m[3][0], m[3][1], m[3][2]);
	}
	inline Vector3 GetDirection() const
	{
		// Note - the following code can be used to double check the vector construction above.
		Mat44 justRot = *this;
		justRot.SetPosition(Vector3(0.f,0.f,0.f));
		Vector3 forward = justRot.Xform(g_forward);
		return forward;
	}
	inline Vector3 GetRight() const
	{
		// Note - the following code can be used to double check the vector construction above.
		Mat44 justRot = *this;
		justRot.SetPosition(Vector3(0.f,0.f,0.f));
		Vector3 right = justRot.Xform(g_right);
		return right;
	}
	inline Vector3 GetUp() const
	{
		// Note - the following code can be used to double check the vector construction above.
		Mat44 justRot = *this;
		justRot.SetPosition(Vector3(0.f,0.f,0.f));
		Vector3 up = justRot.Xform(g_up);
		return up;
	}
	inline Vector3 GetYawPitchRoll() const
	{
		float yaw, pitch, roll;

		pitch = asin(-_32);

		double threshold = 0.001; // Hardcoded constant - burn him, he's a witch
		double test = cos(pitch);

		if(test > threshold) 
		{
			roll = atan2(_12, _22);
			yaw = atan2(_31, _33);
		}
		else 
		{
			roll = atan2(-_21, _11);
			yaw = 0.0;
		}

		return (Vector3(yaw, pitch, roll));
	}
	inline Vector3 GetScale() const
	{
		return Vector3(m[0][0], m[1][1], m[2][2]);
	}
	inline Vector4 Xform(Vector4& v) const
	{
		Vector4 tmp;
		D3DXVec4Transform(&tmp, &v, this);
		return tmp;
	}
	inline Vector3 Xform(Vector3& v) const
	{
		Vector4 tmp(v), out;
		D3DXVec4Transform(&out, &tmp, this);
		return Vector3(out);
	}
	inline Mat44 Inverse() const
	{
		Mat44 out;
		D3DXMatrixInverse(&out, NULL, this);
		return out;
	}

	// Init methods
	inline void BuildTranslation(const Vector3& pos)
	{
		*this = Mat44::g_identity;
		SetPosition(pos);
	}
	inline void BuildTranslation(const float x, const float y, const float z)
	{
		BuildTranslation(Vector3(x,y,z));
	}
	inline void BuildScale(const float x, const float y, const float z)
	{
		*this = Mat44::g_identity;
		m[1][1] = x;
		m[2][2] = y;
		m[3][3] = z;
	}
	inline void BuildRotationX(const float radians) {D3DXMatrixRotationX(this, radians);}
	inline void BuildRotationY(const float radians) {D3DXMatrixRotationY(this, radians);}
	inline void BuildRotationZ(const float radians) {D3DXMatrixRotationZ(this, radians);}
	inline void BuildYawPitchRoll(const float yawRadians, const float pitchRadians, const float rollRadians)
	{
		D3DXMatrixRotationYawPitchRoll(this, yawRadians, pitchRadians, rollRadians);
	}
	inline void BuildRotationQuat(const Quaternion& q) {D3DXMatrixRotationQuaternion(this, &q);}
	inline void BuildRotationLookAt(const Vector3& eye, const Vector3& at, const Vector3& up)
	{
		D3DXMatrixLookAtLH(this, &eye, &at, &up);
	}
};

inline Mat44 operator*(const Mat44& a, const Mat44& b)
{
	Mat44 out;
	D3DXMatrixMultiply(&out, &a, &b);
	return out;
}